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摘要 : 全 向 移动 平台 可 向 任意 方向 直线 运动 而 无 需 事先 旋转 运动 ， 可 在 直线 运动 
的 同时 执行 旋转 运动 ， 机 动 性 及 避 障 性 能 好 。 本 文 介绍 了 基于 NI 公司 myRIO-1900 型 
控制 器 的 误差 消除 型 里 程 计 定位 系统 ， 在 基于 编码 器 、 陀 螺 仪 的 传统 里 程 计 定位 的 基 
础 上 ，3 引 入 红外 传感器 、 超 声波 传感器 ， 通 过 均值 滤波 与 修正 ， 精 确 测量 平台 的 姿态 ， 
并 周期 性 将 累计 误差 清 零 ， 达 到 消除 累计 误差 的 目的 。 该 方法 已 应 用 于 2017 年 世界 技 
能 大 赛 移动 机 器 人 项 目 ， 竞 赛 平 台 运动 控制 效果 良好 ， 满 足 高 速 、 精 确 的 运动 控制 要 
求 。 本 文 提出 的 误差 消除 型 里 程 计 定位 方案 性 价 比 高 ， 实 用 性 强 ， 控 制 精确 ， 应 用 前 
景 广阔 。 
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An Analysis on the Odometer Positioning System 
With Error Elimination 
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Abstract: The omni-directional mobile platform is maneuverable and good at 
obstacle avoidance, as it can move straight in any direction and perform rotational motion 
at the same time. In this paper, an odometer positioning system is introduced with error 
elimination for NI myRIO-1900. Compared to traditional odometer positioning system 
which mainly depends on encoder and gyroscope, infrared sensor and ultrasonic sensor 
are introduced to our system. Through mean filter and periodical clearing cumulative 
error, Our system could measure the posture of the platform accurately. This method has 
been applied in the 2017 the 44th World Skills Competition mobile robotics project. 
The competition result proved that our positioning system has good performance, 
which satisfies the requirement of high speed and precise motion control. The odometer 
positioning System with error elimination presented in this paper is cost-effective and 
practicable, which is a precise control scheme. 
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1 引言 


移动 机 器 人 在 星球 探险 、 电 力 巡 检 、 医 疗 卫 
生 、 工 业 制造 、 国 防 及 公共 服务 领域 的 应 用 越 来 越 
广泛 。 移 动 平台 精确 定位 是 机 器 人 完成 设 定 任务 
的 基本 前 提 中 。 全 向 轮 式 移动 平台 可 在 平面 360° 任 
意 角 度 回转 、 平 移 ， 准 确定 位 ， 灵 活 调 姿 ， 适 于 狭 
小 空间 或 复杂 路 径 移 动 。 全 向 轮 式 移动 平台 定位 方 
法 包括 导航 信 标 定位 、 地 图 匹配 定位 、GPS 定位 、 
北斗 定位 、 视 觉 跟 踪 定 位 和 里 程 计 定位 等 。 不 
同 的 定位 方法 具有 不 同 的 特点 。 导 航 信 标 定位 依 
赖 于 环境 中 事先 添加 的 标志 物 ， 前 期 投入 较 大 ， 
成 本 高 兄 ， 地 图 匹配 定位 精度 高 ， 但 需要 事先 预 
知 环 境地 图 ， 运 算数 据 量 大 ， 要 求 存储 空间 大 ， 对 
处 理 器 要 求 高 ， 不 能 适应 动态 变化 的 环境 ，GPS 定 
位 和 北斗 定位 基于 全 球 卫星 定位 系统 "'"， 定 位 精度 
低 ， 室 内 可 能 定位 失败 ;视觉 跟 踊 定位 精度 较 高 ， 
但 实时 性 较 差 ， 存在 跟踪 目标 遮挡 导致 的 目标 丢失 
问题 "里程 计 定位 采用 惯性 传感器 ""， 存 在 
误差 积累 "。 误 差 消 除 型 里 程 计 定位 方法 可 周期 
性 校准 移动 平台 的 位 姿 ， 消 除 累计 误差 ， 可 获得 全 
方位 运动 效果 。 


2 ”全 向 平台 运动 学 模型 与 里 程 计 定位 分 析 


2.1 全 向 平台 运动 学 模型 

全 向 移动 平台 "" 由 三 个 全 向 轮 组 成 ， 轮 子 均 
匀 分 布 ， 互 差 120° 角 ， 如 图 1 所 示 。 全 向 移动 平台 
不 存在 不 完整 约束 ， 可 向 任意 方向 直线 运动 而 无 需 
事先 旋转 运动 ， 可 在 直线 运动 的 同时 执行 旋转 运动 ， 
具有 运动 灵活 的 特点 。 
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图 1 全 向 移动 平台 


Fig.l Omni-directional mobile platform 


移动 平台 的 运动 学 简化 模型 如 图 2 所 示 ,，XOY 
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为 世界 坐标 系 ，xoy 为 平台 坐标 系 ， 两 坐标 系 间 的 
夹 角 为 B65，3 个 轮子 分 别 标记 为 i、j、Kk， 线 速度 标 
记 为 矿 、 万 、 了 瓜 ， 平 台 旋 转 的 角速度 为 w， 轮 子 到 
平台 中 心 点 O 的 距离 为 有 。 


MM 


图 2 移动 平台 的 运动 学 简化 模型 
Fig.2 Kinematic model of mobile platform 
在 平台 坐标 系 中 ， 平 台 沿 两 个 坐标 轴 方 向 的 线 速 
度 为 VY、 人 中 与 厂矿 反之 间 的 关系 式 为 


3 3 3 
Vy 及 
vy,|=| 0 3 3 及 (1) 
3 3 
| 
3R 3R 3R 
在 世界 坐标 系 中 ， 平 台 沿 两 个 坐标 轴 方 向 的 线 


速度 为 Vi Vy Vs, Vy CO 与 Vs Vy 之 间 的 关 
系 式 为 


V cos® -sin® 0 | | 
太 |=| sin® cos® 0 ly, (2) 
Ee 0 0 1|, 


机 器 人 的 当前 姿态 (xz， 思 ，9) 与 初始 姿态 
(0 ]0， 060) 之 间 的 关系 式 为 


yl=| 0 At 0 liy, (3) 


式 中 ，At 为 机 器 人 运行 时 间 ; x;、yi;、0;(i=0,1) 分 
别 为 移动 平台 位 姿 的 x 轴 坐 标 值 、y 轴 坐 标 值 和 角 
度 值 。 
2.2 里 程 计 定位 分 析 

里 程 计 定位 方法 基于 里 程 计 惯 性 传感器 相对 于 
初始 状态 数据 的 变化 量 来 计算 移动 平台 的 当前 位 姿 。 
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这 种 相对 定位 方法 按照 移动 平台 的 运动 学 模型 来 推 
测 自身 的 运动 轨迹 ， 不 需要 外 界 提 供 平 台 环 境 信息 ， 
具有 自 适 应 的 优点 。 然 而 随 着 运动 轨迹 的 推移 ， 会 
出 现 定位 误差 积累 现象 。 平 台 的 轮子 直径 、 轴 距 的 
差异 与 安装 精度 引起 的 系统 误差 具有 规律 性 ， 平台 
运动 过 程 中 打滑 、 锁 死 以 及 轮子 与 地 面 的 不 连续 接 
触 引起 的 非 系 统 误差 具有 不 确定 性 特点 。 
3 ”误差 消除 型 里 程 计 定位 系统 设计 
传统 的 里 程 计 方法 采用 电机 编码 器 和 陀螺 仪 等 
惯性 传感器 ， 存 在 误差 积累 。 误 差 消除 型 里 程 计 系 
统 采 用 NI 公司 的 myRIO-1900 型 控制 器 ， 在 传统 里 
程 计 定位 系统 的 基础 上 ， 硬 件 上 增加 超声 波 、 红 外 
测 距 传感器 ， 控 制 软 件 中 增加 位 姿 校准 子 程序 ， 定 
期 对 移动 平台 的 位 姿 进 行 校准 ， 将 安装 精度 与 轮子 
与 地 面 接触 不 连续 等 原因 造成 的 累计 误差 请 零 ， 之 
后 继续 完成 预定 任务 。 系 统 硬件 结构 如 图 3 所 示 。 


QTI 
> 电机 
由 ” 及 其 驱动 1 
PING ”myRIO-1900| 一 > 及 a 
电机 
Camera [== 一 及 其 驱动 3 
旋转 编码 器 


3 ”控制 系统 硬件 组 成 


Fig.3 Hardware composition of control system 


3.1 硬件 电路 设计 

全 向 移动 平台 主要 电气 元 器 件 见 表 1。 红 外 传 
感 器 与 超声 波 传感器 结合 使 用 测量 移动 平台 的 直角 
坐标 值 ， 在 事先 确定 的 固定 点 与 绝对 坐标 值 进行 比 
较 ， 得 出 误差 值 ，myRIO-1900 型 控制 器 发 出 校准 
信号 ， 经 功率 放大 后 驱动 电机 进行 校正 ， 直 至 误差 
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清 零 。 系 统 接线 图 如 图 4 所 示 ， 其 中 ， 传 感 器 接线 
图 如 图 5 所 示 。 


SV 5V 
Vee Vee 
myRIO-1900 
Vo Vo 
| B/AIO 
GND GND 
GP2Y0A21YKOF GP2Y0A21YKOF B/AIl 


SV 5V B/DIOO 


VeceT Vcc 

Se Se lk® B/DIOIl 
PING ))) PING ))) 

GND | GND 


lkQ 四 


图 5 传感器 接线 图 


Fig.39 Senor wiring diagram 


平台 行走 结构 采用 3 台 直 流 减 速 电 机 进行 驱动 ， 
电机 需要 正 反 向 运转 ， 采 用 理 桥 PWM 驱动 。 瓦 桥 
PWM 驱动 芯片 为 A4973 型 ， 内置 PWM 电流 控制 ， 
具有 快速 和 慢 速 两 种 电流 衰减 模式 ， 驱 动 电流 为 
1.5A， 具 有 过 电流 和 欠 电 压 等 完善 的 保护 功能 。 电 
机 驱动 电路 如 图 6 所 示 , PWM1 信号 来 自 myRIO- 
1900 型 控制 器 接口 B/DIO8 (PWM0), Ml+、M1- 
接 1]# 直 流 电机 ，AO1 可 设置 电机 限 流 保护 点 ， 
SENSE1 将 电机 电流 上 报 myRIO-1900 型 控制 器 。 
电机 正常 驱动 电流 路 径 ( 正 向 旋转 )、PWM 电流 控 
制 快速 衰减 电机 电流 路 径 和 慢 速 衰减 电机 电流 路 径 
如 图 7 所 示 。 
3.2 软件 设计 

全 向 移动 平台 程序 控制 采用 LabVIEW2015， 程 
序 采用 模块 化 设计 方法 。 主 要 模块 包括 陀螺 仪 数 据 
采集 与 定位 计算 、 电 机 速度 采集 与 困 环 控制 、 红 外 
和 超声 传感器 数据 采集 与 位 姿 校 准 。 

myRIO-1900 型 控制 器 通过 采集 陀螺 仪 数据 得 
到 平台 当前 的 旋转 角度 ， 采 集 平 台 3 台电 机 的 编码 
值得 到 平台 当前 的 直角 坐标 值 ， 进 而 得 到 平台 的 运 


表 1 主要 电气 元 器 件 


Tab.1 List ofelectrical components 


序号 名 称 规格 型 号 性 能 指标 数量 /个 
1 控制 器 NI，myRIO-1900 型 双核 ARM Cortex-A9，Xilinx FPGA | 
2 直流 减速 电机 TETRI，V2 型 12V，2.26N . m, 152r/min 3 
3 旋转 编码 器 US Digital E4P 型 1 440 PPR 3 
4 电机 驱动 板 A4973 型 两 路 ， 驱 动 电流 1.5A 2 
5 红外 传感器 SHARP，GP2Y0A21YKOF 型 量程 : 100 ~ 800mm 1 
6 超声 波 传感器 PARALLAX，28015# 型 量程 : 20 ~ 3 000mm 2 
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+5V Vbbl 
Ce 100nF 
GND 2 10V vv a 
+3.3V GND 
On 
Ro 3 3 
+3.3V Re IkQ L! SNR6045E-100 
MODE1 | No Load 
， a 10kQ 14 WDE 本 |、10 Ar MI+ 
40 
[Nees > PHASE H 
PWMI Ri eS L, SNR6045E-100 
+5V Ro, IOkG ie MAE 15 | M1- 
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10uH 
3 IC1 
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RC 2 2 REF 
[i 470pF 33KO 呈 SN SENSE1 
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1% ||R» Se R, 
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VREF2V5 R00 GND lal alsa GND 
AOI Rn GND 
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6 电机 了 驱动 电路 图 
Fig.6 Motor driver circuit diagram 
(开始 ) 开始 
PE 
写 红外 传感器 数据 X 写 超声 波 数据 和 
Y Y 
| Y=27.5/¥ 7= 声速 2 
Y Y 
| 均值 让 均值 滤波 
Y Y 
| 读 距 离 ! 值 读 距离 7 值 
VY 由 
结束 结束 


一 = 一 :一 * 


a 
1 Drive current (Normal) 


(a) 红外 传感器 数据 采集 
数据 采集 流程 图 


Recirculation (Slow decay) 
---> 


R&R [| De 
7 电机 电流 路 径 图 


Fig.7 Motor current path diagram 


坐标 和 速度 。 运 动 导航 根据 当前 坐标 和 目标 坐 
S， 得 到 补偿 距离 和 速度 ， 经 过 速度 环 和 位 置 环 控 
采用 PID 算法 ， 实 现 平台 式 轴 、 了 了 轴 直 线 运 动 。 


平台 采集 红外 和 超声 传感器 数据 信息 ， 并 定期 对 平 
台式 轴 、 了 轴 两 个 方向 进行 位 置 校准 ， 红 外 和 超声 
传感器 数据 采集 流程 图 如 图 8 所 示 ， 位 姿 校准 示意 


图 如 图 


4 ”实验 结果 


9 所 示 ， 流 程 图 如 图 10 所 示 。 


取 100mm、200mm、300mm 三 个 实际 距离 ， 标 


图 8 


(b) 超声 传感器 数据 采集 


Fig.8 Data collection fow chart 


9 ”位 姿 校准 示意 图 


Fig.9 Position and posture calibration 


记 为 距离 1、 距离 2、 距 离 3， 全 向 移动 平台 采用 
“Analog input”VI 读 取 红 外 传感器 数据 ， 得 到 三 


个 测量 数据 ， 与 对 应 实际 距离 进行 对 比 ， 如 图 11a 
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开始 
Y 


超声 波 1 数 据 采 集 ， 得 到 y， 
超声 波 2 数 据 采 集 ， 得 到 y， 


红外 1 数据 采集 ， 得 到 x 
红外 2 数据 采集 ， 得 到 x 


图 10 位 姿 校准 流程 图 


Fig.10 Position and posture calibration flow chart 


所 示 ， 测 量 数据 偏差 范围 为 12.36 ~ 15.21mm， 波 
动 为 2.85mm; 对 测量 数据 进行 均值 滤波 后 ， 与 
对 应 实际 距离 进行 对 比 ， 如 图 11b 所 示 ， 测 量 数 
据 偏 差 范 围 为 9.23 ~ 10.54mm， 波 动 为 1.31mm。 
可 见 ， 均值 滤波 后 ， 偏 差 基 本 恒定 ， 数 据 波动 减 
小 。 在 平台 位 置 校准 中 进行 偏差 修正 ， 测 量 误差 
为 土 0.66mm。myRIO-1900 型 控制 器 的 10 口 数据 
由 FPGA 采集， 依然 取 100mm、200mm、300mm 
三 个 实际 距离 ， 标 记 为 距离 1、 距离 2、 距 离 3， 
LabVIEW 实时 模块 (LabVIEW Real-Time，RT) 查 
询 访问 FPGA 时 ， 测 量 值 与 实际 值 对 比如 图 12a 所 
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(b) 采用 均值 滤波 
图 11 红外 传感器 测量 值 与 实际 值 对 比 


Fig.ll Comparison of measured and actual values of 


infrared sensors 


示 ， 测 量 数 据 偏差 范围 为 21.35 ~ 24.28mm， 波 动 
为 2.93mm; FPGA 直接 读 取 IO 口 数据 时 ,测量 值 
与 实际 值 对 比如 图 12b 所 示 ， 测 量 数据 偏差 范围 
为 6.34 ~ 8.26mm， 波 动 为 1.92mm。 可 见 ，FPGA 
直接 读 取 IO 口 数据 时 ， 测 量 数值 与 实际 值 的 偏差 
明显 减 小 ， 数 据 波动 减 小 ， 进 行 偏差 修正 和 均值 滤 
波 后 ， 测 量 误差 为 土 0.96mm。RT 查询 访问 FPGA 
时 ,测量 值 波 动 大 ， 且 与 实际 值 偏差 大 的 原因 是 超 
声波 传感器 的 触发 与 读 取 时 间 长 ， 引 起 数字 信号 的 
丢失 和 延迟 。 将 基于 上 述 算法 的 红外 传感器 与 超声 
波 传感器 应 用 于 全 向 移动 平台 ， 让 平台 进行 多 次 向 
前 直线 运动 10m， 测 试 数 据 显 示 : 平台 在 前 行 过 程 
中 ， 运 行 10m 平均 累计 误差 为 土 50mm， 在 移动 平 
台 目 标 管 理 任 务 中 ， 该 误差 尚 可 以 接受 。 为 了 移动 
平台 执行 目标 管理 任务 时 更 精准 ， 让 全 向 移动 平台 
沿 着 如 图 13 所 示 的 路 径 前 行 ， 并 在 4、B8、C、 DD、 
巨 五 点 进行 移动 平台 姿态 校准 与 误差 清 零 ， 将 移动 
平台 位 姿 误 差 与 实际 行进 距离 关系 绘 成 曲线 ， 如 图 
14 所 示 。 观 察 移动 平台 位 姿 误差 值 与 实际 行进 距离 
之 间 的 关系 曲线 ， 发 现 曲线 分 为 四 段 ， 每 段 曲 线 随 
着 移动 平台 前 行 ， 位 置 误差 不 断 积累 ， 第 三 段 曲线 
累计 误差 最 大 ， 达 到 22mm， 为 平台 最 大 误差 ;在 
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(b) 采用 FPGA 读 取 
图 12 超声波 传感器 测量 值 与 实际 值 对 比 


Fig.12 Comparison of measured and actual values of 


ultrasonic sensor 
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喜 


B.C 万 


13 ”测试 路 径 规划 
Fig.13 Diagram of test path planning 


4、B、C、 DD 点 移动 平台 进行 误差 清 零 ， 标 红 的 部 
分 误差 有 阶 跃 现象 ， 是 因为 移动 平台 转弯 ， 轮 子 与 
地 面 发 生 打 请 所 致 。 


5 ”结束 语 


本 文 提出 一 种 改进 型 里 程 计 定位 方法 ， 在 利用 
编码 器 、 陀 螺 仪 的 传统 里 程 计 定位 的 基础 上 ， 引 入 
红外 传感器 和 超声 波 传感器 ， 通 过 均值 滤波 与 修正 ， 
精确 测量 平台 直角 坐标 与 位 姿 ， 周 期 性 清除 累计 误 
差 ， 达到 消除 累计 误差 的 目的 。2017 年 10 月 , 将 上 
述 方法 制作 的 移动 机 器 人 应 用 于 世界 技能 大 赛 移动 
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14 ”移动 平台 位 姿 误差 值 与 行进 路 程 试验 对 照 图 


Fig.l4 Comparison of mobile platform error and move distance 


机 器 人 赛场 上 ， 运 动 控制 效果 良好 ， 可 靠 性 高 ， 
满足 比赛 定位 需求 。 

该 方法 适 于 固定 场地 、 周 期 性 循环 应 用 场合 ， 
如 电力 巡 检 所 、 医 疗 服务 、 库 房管 理 等 。 本 文 提出 
的 改进 型 里 程 计 定位 方法 为 低 成 本 机 器 人 定位 方法 ， 
性 价 比 高 ， 实 用 性 强 ， 在 全 向 移动 机 器 人 定位 、 导 
航 控 制 方面 具有 推广 价值 。 
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